using System;
using System.Drawing;
using System.Drawing.Imaging;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// An Occupancy Grid of doubles.
    /// </summary>
    public class Map : ICloneable
    {
        private int _width;
        private int _height;
        private double _scale;
        private double _threshold;
        private double[,] _occupancy;

        /// <summary>
        /// The number of pixels across.
        /// </summary>
        public int Width
        {
            get { return _width; }
            set { _width = value; }
        }

        /// <summary>
        /// The number of pixels down.
        /// </summary>
        public int Height
        {
            get { return _height; }
            set { _height = value; }
        }

        /// <summary>
        /// The number of centimeters per pixel.
        /// </summary>
        public double Scale
        {
            get { return _scale; }
            set { _scale = value; }
        }

        /// <summary>
        /// The level above which is considerred occupied.
        /// </summary>
        public double Threshold
        {
            get { return _threshold; }
            set { _threshold = value; }
        }

        /// <summary>
        /// The grid of doubles representing probability.
        /// </summary>
        public double[,] Occupancy
        {
            get { return _occupancy; }
            set { _occupancy = value; }
        }

        /// <summary>
        /// Creates an occupancy grid.
        /// </summary>
        /// <param name="width">The number of pixels across.</param>
        /// <param name="height">The number of pixels down.</param>
        /// <param name="threshold">The level above which is considered occupied.</param>
        /// <param name="scale">The number of centimeters per pixel.</param>
        public Map(int width, int height, double threshold, double scale)
        {
            _width = width;
            _height = height;
            _threshold = threshold;
            _scale = scale;
            _occupancy = new double[_width, _height];
        }

        /// <summary>
        /// Tells you that the pixel is above the map's threshold.
        /// </summary>
        /// <param name="x">The number of pixels from the left.</param>
        /// <param name="y">The number of pixels form the top.</param>
        /// <returns>True if occupied.</returns>
        public bool IsOccupied(int x, int y)
        {
            if (x > 0 && y > 0 && x < _width && y < _height)
            {
                return _occupancy[x, y] > _threshold; // should convert to prob first
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// Tells you that the pixel is above the map's threshold.
        /// </summary>
        /// <param name="p">The point you want to test.</param>
        /// <returns>True if occupied.</returns>
        public bool IsOccupied(Point p)
        {
            return IsOccupied((int)Math.Round(p.X /_scale),(int)Math.Round( p.Y/_scale));
        }

        /// <summary>
        /// Adds an array of distance readings to the map, at the location of the robot.
        /// </summary>
        /// <param name="state"></param>
        /// <param name="readings"></param>
        public void AddReadings(Pose2D state, DistanceReading[] readings)
        {
            foreach(DistanceReading reading in readings) {
               AddReading(state, reading);
            }
        }

        /// <summary>
        /// Adds a distance reading to the map, at the location of the robot.
        /// </summary>
        /// <param name="state">The X,Y,Theta of the robot</param>
        /// <param name="reading">A desciption of the type of sensor, its location relative to the robot and the current distance.</param>
        public void AddReading(Pose2D state, DistanceReading reading)
        {
            // calculate origin and angle of each reading and pass the map to the cone model for updating
            double readingOriginX = state.X + (Math.Cos(state.Theta) * reading.X) - (Math.Sin(state.Theta) * reading.Y);
            double readingOriginY = state.Y + (Math.Sin(state.Theta) * reading.X) + (Math.Cos(state.Theta) * reading.Y);

            DistanceReading adjustedReading = new DistanceReading(readingOriginX, readingOriginY, state.Theta + reading.Theta, reading.Distance, reading.Beam, reading.Cone);
            adjustedReading.UpdateMap(ref _occupancy, _scale);
        }

        // Blanks out the robots current position
        public void ClearRobotPosition(Pose2D state)
        {
            int x = (int)Math.Round(state.X / _scale);
            int y = (int)Math.Round(state.Y / _scale);
            if (x > 0 && y > 0 && x < _width && y < _height)
            {
                _occupancy[x, y] = -2.5; // in log odds, about zero prob.
            }
        }

        /// <summary>
        /// A measure of the disorder in the map
        /// </summary>
        /// <returns></returns>
        public double GetEntropy()
        {
            double tot = 0;
            for(int x=0; x<_width; x++)
            {
                for(int y =0; y<_height; y++)
                {
                    double prob = 1 - (1/(1 + Math.Pow(Math.E, _occupancy[x, y])));
                    tot += prob*Math.Log(prob) - (1 - prob)*Math.Log(1 - prob);
                }
            }
            return tot / (_width * _height);
        }

        /// <summary>
        /// Loads a grayscale image where darkness represents occupancy.
        /// </summary>
        /// <param name="image">a map</param>
        public void LoadImage(Image image)
        {
            Bitmap b = ((Bitmap)image);
            _width = image.Width;
            _height = image.Height;
            Console.Out.WriteLine("Loading image: width,height " + Width + "," + Height);
            _occupancy = new double[_width,_height];
            BitmapData data = b.LockBits(new Rectangle(0, 0, _width, _height), ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
            unsafe
            {
                byte* imgPtr = (byte*)(data.Scan0);
                for (int i = 0; i < data.Height; i++)
                {
                    for (int j = 0; j < data.Width; j++)
                    {
                        double val = (765 - (imgPtr[0] + imgPtr[1] + imgPtr[2])) * (1.0 / 765.0);
                        
                        _occupancy[j, i] = Math.Max(-2.5,Math.Min(Math.Log(val/(1 - val)),2.5));
                        
                        imgPtr += 3;
                    }
                    imgPtr += data.Stride - data.Width * 3;
                }
            }
            b.UnlockBits(data);
            Console.Out.WriteLine("Loaded Image");
        }

       /// <summary>
       /// Makes a copy of this map. Don't use this often on big maps.
       /// </summary>
       /// <returns></returns>
        public object Clone()
        {
            Map m = new Map(_width, _height, _threshold, _scale);
            m.Occupancy = (double[,])_occupancy.Clone(); // deep enough?
            return m;
        }
    }
}
